/*
 * @brief: 步进电机驱动
 * @date: 2015.09.09
 *
 * ChangeLog
 * Date             Author          Notes
 * 2015.12.16       Herbert         修改电机顺序
 *                                  传进来的电机顺序统一按照中间层的顺序q1,q2,h1,h2,q3,q4,h3,h4,qxk,hxk排列
 *                                  将中间层的顺序转换为下位机的顺序
 * 2016.01.13       Herbert         增加3G机器电机顺序的转换
 * 2016.02.29       Herbert         增加电机个数参数
 */
#include "stepmotor.h"
#include "dispatch.h"
#include <assert.h>
#include "act_types.h"
#include <string.h>

#define MDIR            0x29
#define FIND_ZERO1      0x8000
#define FORWARD         0x2000
#define REVERSE         0x0000

static void stepmotor_convert_pos(short *pos_in, short *pos_out, int num)
{
    assert(pos_in != NULL);
    assert(pos_out != NULL);

    int i = 0;
    short pos_in_tmp[10] = { 0 };

    memcpy(pos_in_tmp, pos_in, 10 * sizeof (short));

    for (i = 0; i < 4; i++)
    {
        pos_out[i] = pos_in_tmp[i];
    }

    /* 上位机转换到下位机 */
    pos_out[4] = pos_in_tmp[8];
    for (i = 0; i < 4; i++)
    {
        pos_out[5 + i] = pos_in_tmp[4 + i];
    }
    pos_out[9] = pos_in_tmp[9];
}

void setup_reset_msg(uchar *data, short *find_zero, int num)
{
    int i = 0;

    for (i = 0; i < num; i++)
    {
        ushort cmd = 0;
        if (find_zero[i])
        {
            cmd = 0xA000;
            printf("恢复对象:%d\n",i);
        }

        data[i * 2] = cmd & 0x00FF;
        data[i * 2 + 1] = (cmd & 0xFF00) >> 8;
    }
}

void setup_stepmotor_msg(uchar *data, short *delta, short *find_zero, int num)
{
    int i = 0;

    for (i = 0; i < num; i++)
    {
        ushort cmd = 0;
        if (delta[i] > 0)
        {
            cmd = 0x4000 | delta[i];
        }
        else
        {
            cmd = 0x5000 | -delta[i];
            if (find_zero[i])
            {
                cmd = 0x8000 | -delta[i];
            }
        }

        data[i * 2] = cmd & 0x00FF;
        data[i * 2 + 1] = (cmd & 0xFF00) >> 8;
    }
}

int stepmotor_exec(MH_UDP *pudp, int ctype, short *delta, short *find_zero, int num)
{
    assert(delta != NULL);
    assert(find_zero != NULL);

    CAN_MSG msg = { 0, 0, { 0 } };
    int ret = 0;

    stepmotor_convert_pos(delta, delta, num);
    stepmotor_convert_pos(find_zero, find_zero, num);

    /* 第一次消息 */
    msg.id = (ctype == LEFT_CARRIAGE) ? ID_MOTOR1 : ID_MOTOR1_R;
    msg.len = 8;

    memset(msg.data, 0, 8);
    setup_stepmotor_msg(msg.data, delta, find_zero, 4);
    ret = dispatch_msg(pudp,&msg, NULL);

    if (ret < 0)
    {
        return ret;
    }
    /* 第二次消息 */
    msg.id = (ctype == LEFT_CARRIAGE) ? ID_MOTOR2 : ID_MOTOR2_R;
    msg.len = 8;

    memset(msg.data, 0, 8);
    setup_stepmotor_msg(msg.data, delta + 4, find_zero + 4, 4);

    ret = dispatch_msg(pudp, &msg, NULL);
    if (ret < 0)
    {
        return ret;
    }

    /* 第3次消息 */
    msg.id = (ctype == LEFT_CARRIAGE) ? ID_MOTOR3 : ID_MOTOR3_R;
    msg.len = 8;

    memset(msg.data, 0, 8);
    if (num > 10)
    {
        setup_stepmotor_msg(msg.data, delta + 8, find_zero + 8, 4);
    }
    else
    {
        setup_stepmotor_msg(msg.data, delta + 8, find_zero + 8, 2);
    }
    ret = dispatch_msg(pudp, &msg, NULL);
    return ret;
}

int stepmotor_reset(MH_UDP *pudp, int ctype, short *find_zero, int num)
{
    assert(find_zero != NULL);

    CAN_MSG msg = { 0, 0, { 0 } };
    int ret = 0;

    stepmotor_convert_pos(find_zero, find_zero, num);

    /* 第一次消息 */
    msg.id = (ctype == LEFT_CARRIAGE) ? ID_MOTOR1 : ID_MOTOR1_R;
    msg.len = 8;

    memset(msg.data, 0, 8);

    setup_reset_msg(msg.data, find_zero, 4);

    ret = dispatch_msg(pudp,&msg, NULL);
    if (ret < 0)
    {
        return ret;
    }
    /* 第二次消息 */
    msg.id = (ctype == LEFT_CARRIAGE) ? ID_MOTOR2 : ID_MOTOR2_R;
    msg.len = 8;

    memset(msg.data, 0, 8);
    setup_reset_msg(msg.data, find_zero + 4, 4);

    ret = dispatch_msg(pudp,&msg, NULL);
    if (ret < 0)
    {
        return ret;
    }

    /* 第3次消息 */
    msg.id = (ctype == LEFT_CARRIAGE) ? ID_MOTOR3 : ID_MOTOR3_R;
    msg.len = 8;

    memset(msg.data, 0, 8);
    if (num > 10)
    {
        setup_reset_msg(msg.data, find_zero + 8, 4);
    }
    else
    {
        setup_reset_msg(msg.data, find_zero + 8, 2);
    }
    ret = dispatch_msg(pudp, &msg, NULL);
    return ret;
}

